﻿using System;
using System.Device.Location;
using Microsoft.Xna.Framework;

namespace Tequila.AugmentedReality
{
    internal static class ProjectionArithmetics
    {
        private static readonly GeoCoordinate NorthLocation = new GeoCoordinate(90, 0);

        public static Vector3 CalculateLocationVector3(GeoCoordinate fromLocation, GeoCoordinate toLocation)
        {
            var course = GetCourse(fromLocation, toLocation);
            var distanceInMeters = fromLocation.GetDistanceTo(toLocation);
            var vector = AngleToVector(course, distanceInMeters);
            return vector;
        }

        public static double ToRadians(this double degrees)
        {
            return degrees * Math.PI / 180;
        }

        public static double ToDegress(this double radians)
        {
            return radians * 180 / Math.PI;
        }

        /// <summary>
        /// Polar -> Cartesian coordinate conversion. X-axis points directly. Z-axis perpendicularly
        /// </summary>
        public static Vector3 AngleToVector(double angle, double radius)
        {
            float num = MathHelper.ToRadians((float)(angle - 90.0));
            return new Vector3((float)(radius * Math.Cos(num)), 0.0f, (float)(radius * Math.Sin(num)));
                
        }

        /// <summary>
        /// Calculates angle North->currentLocation->targetLocation
        /// </summary>
        /// <returns>Values range (-180..180). -179 for South and a bit West. 179 for South and a bit East. </returns>
        public static double GetCourse(GeoCoordinate currentLocation, GeoCoordinate targetLocation)
        {
            var ab = CalculateAngularDistance(currentLocation, NorthLocation);
            var ac = CalculateAngularDistance(currentLocation, targetLocation);
            var bc = CalculateAngularDistance(NorthLocation, targetLocation);

            //calculate angle between three point on sphere
            //http://gis-lab.info/qa/angles-sphere.html
            var bacAngle = Math.Acos((Math.Cos(bc) - Math.Cos(ac) * Math.Cos(ab)) / (Math.Sin(ac) * Math.Sin(ab)));

            var course = bacAngle.ToDegress();

            if (targetLocation.Longitude < currentLocation.Longitude)
            {
                course *= -1;
            }

            return double.IsNaN(course) ? 0 : course;
        }

        public static double CalculateAngularDistance(GeoCoordinate coordinate1, GeoCoordinate coordinate2)
        {
            var lat1InRadians = coordinate1.Latitude.ToRadians();
            var long1InRadians = coordinate1.Longitude.ToRadians();

            var lat2InRadians = coordinate2.Latitude.ToRadians();
            var long2InRadians = coordinate2.Longitude.ToRadians();

            var deltaLongitude = long2InRadians - long1InRadians;

            //http://gis-lab.info/qa/great-circles.html
            var distance = Math.Acos(Math.Sin(lat1InRadians) * Math.Sin(lat2InRadians) + Math.Cos(lat1InRadians) * Math.Cos(lat2InRadians) * Math.Cos(deltaLongitude));
            return distance;
        }
    }
}
